import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os
from launch.actions import DeclareLaunchArgument


def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(
        package="robot_description"
    ).find("robot_description")
    default_model_path = os.path.join(pkg_share, "resource/xacro/combined_robot.xacro")
    default_rviz_config_path = os.path.join(pkg_share, "rviz/urdf_config.rviz")
    use_rviz = LaunchConfiguration("use_rviz")
    use_joint_state_publisher_gui = LaunchConfiguration("use_joint_state_publisher_gui")
    namespace = LaunchConfiguration("namespace")
    use_sim_time = LaunchConfiguration("use_sim_time")
    declare_model_path_cmd = DeclareLaunchArgument(
        "model",
        default_value=default_model_path,
        description="Absolute path to robot urdf file",
    )
    declare_use_rviz_cmd = DeclareLaunchArgument(
        "use_rviz", default_value="True", description="Whether to start RViz"
    )
    declare_use_joint_state_publisher_gui_cmd = DeclareLaunchArgument(
        "gui",
        default_value="True",
        description="Whether to start joint_state_publisher",
    )
    declare_rviz_config_cmd = DeclareLaunchArgument(
        "rvizconfig",
        default_value=default_rviz_config_path,
        description="Full path to the RVIZ config file to use",
    )
    declare_namespace_cmd = DeclareLaunchArgument(
        "namespace",
        default_value="",
        description="Top-level namespace",
    )

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        "use_sim_time",
        default_value="True",
        description="Use simulation (Gazebo) clock if true",
    )

    declare_robot_name_cmd = DeclareLaunchArgument(
        "robot_name",
        default_value="combined_robot",
        description="The file name of the robot xmacro to be used",
    )
    robot_state_publisher_node = launch_ros.actions.Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[
            {"robot_description": Command(["xacro ", LaunchConfiguration("model")])}
        ],
    )
    joint_state_publisher_node = launch_ros.actions.Node(
        package="joint_state_publisher",
        executable="joint_state_publisher",
        name="joint_state_publisher",
        condition=launch.conditions.UnlessCondition(LaunchConfiguration("gui")),
    )
    joint_state_publisher_gui_node = launch_ros.actions.Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui",
        name="joint_state_publisher_gui",
        condition=launch.conditions.IfCondition(LaunchConfiguration("gui")),
        parameters=[{
        "zeros": {
            "j1": 0.0,
            "j2": -1.5712,
            "j3": 1.5712,
            "j4": 0.0,
            "j5": 0.0,
            "j6": 0.0
        }
    }]
    )
    rviz_node = launch_ros.actions.Node(
        condition=launch.conditions.IfCondition(use_rviz),
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", LaunchConfiguration("rvizconfig")],
        parameters=[{"use_sim_time": use_sim_time}],
    )
    ld = launch.LaunchDescription()
    ld.add_action(declare_model_path_cmd)
    ld.add_action(declare_use_rviz_cmd)
    ld.add_action(declare_use_joint_state_publisher_gui_cmd)
    ld.add_action(declare_rviz_config_cmd)
    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_robot_name_cmd)

    ld.add_action(robot_state_publisher_node)
    ld.add_action(joint_state_publisher_node)
    ld.add_action(joint_state_publisher_gui_node)
    ld.add_action(rviz_node)
    return ld
    